#ifndef HITSZ_DIP_EX3_HOUGH_H
#define HITSZ_DIP_EX3_HOUGH_H
#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
// #include <chrono>

#include <opencv2/opencv.hpp>

namespace dip{
    /***
     * @brief 霍夫直线检测
     * @param src
     * @param lines，存放点
     * @param rou_step,极径精度(放弃)
     * @param theta_step,极角精度（放弃）
     */ 
    void HoughLines(const cv::Mat &src,std::vector<cv::Vec2f>& lines,int threshold)
    {
        int w = src.cols;
        int h = src.rows;
        int max_rou = static_cast<int>(sqrt(w*w + h*h));
        int size = 2*max_rou;
        int offset = max_rou;
        cv::Mat pole_axis(size,181,CV_8UC1,cv::Scalar(0));//极坐标图像
        auto data_ptr = src.ptr<uchar>();

        int angle,p;
        for (int i=0;i<h;i++)
        {
            for(int j=0;j<w;j++)
            {
                if (*data_ptr == 0)
                {
                    data_ptr++;
                    continue;
                }
                else
                { //累加
                    for(angle=0;angle<181;angle++)
                    {
                        p = j*cos(angle*CV_PI/180) + i *sin(angle*CV_PI/180)+offset;
                        pole_axis.at<uchar>(p,angle)++;
                    }
                    data_ptr++;
                }
            }
        }

        lines.clear();

        //遍历极坐标
        for (int i=0;i<size;i++)
        {
            for (int j=0;j<181;j++)
            {
                if(pole_axis.at<uchar>(i,j) >= threshold)
                { //如果大于阈值
                    lines.emplace_back(cv::Vec2f({i - offset,j*CV_PI/180}));
                }
            }
        }
    }

    /**
     * @brief 霍夫圆检测
     * @param edge，边缘图像
     * @param circles,[x,y,r]
     * @param minRadius，最小半径，单位为pixel
     * @param maxRadius，最大半径，单位为pixel
     * @note 只考虑圆心在图像中的circle
     */ 
    void HoughCircle(const cv::Mat &edge,std::vector<cv::Vec3f>& circles,int minRadius,int maxRadius,int threshold)
    {
        int w = edge.cols;
        int h = edge.rows;
        int channel = maxRadius - minRadius + 1;
        cv::Mat pole_axis = cv::Mat::zeros(h,w,CV_32FC(channel));//极坐标图像(x_center,y_center,r)
        auto data_ptr = edge.ptr<uchar>();

        int angle,p;
        for (int i=0;i<h;i++)
        { //y
            for(int j=0;j<w;j++)
            { //x
                if (*data_ptr == 0)
                { //如果不是边缘点
                    data_ptr++;
                    continue;
                }
                else
                { //边缘点
                    for(int r=minRadius;r<maxRadius+1;r++)
                    { //遍历r
                        for(int angle=0;angle<181;angle+=10)
                        { //遍历圆心角
                            int x_center = j - r*cos(angle*CV_PI/180) ;
                            int y_center = i -r* sin(angle*CV_PI/180);
                            if (x_center < 0|| x_center > w){
                                continue;
                            } 
                            else if(y_center < 0|| y_center > h)
                            {
                                continue;
                            }
                            else{
                                auto pole_ptr = pole_axis.ptr<float>(y_center);//只能走指针了，一般at会报错
                                pole_ptr[x_center*channel + r -minRadius] ++;
                                // pole_axis.at<uchar>(x_center,y_center,r-minRadius)++;
                            }
                        }
                    }
                    data_ptr++;
                }
            }
        }

        circles.clear();

        //遍历极坐标
        for (int i=0;i<h;i++)
        { //y
            auto pole_ptr = pole_axis.ptr<float>(i);
            for (int j=0;j<w;j++)
            { //x,
                for(int r=0;r<channel;r++)
                {
                    // std::cout<<pole_ptr[j*channel + r]<<std::endl;
                    if(pole_ptr[j*channel + r]>= threshold)
                    { //如果大于阈值
                        circles.emplace_back(cv::Vec3f({i,j,r+minRadius}));
                    }
                }
            }
        }//end for update circles

    }
}
#endif